#pragma once

#include "rs485_v3.hpp"
#include "agile_modbus_rtu_master.hpp"
#include <atomic>
#include "yj_time.hpp"
#include <cmath>

class zdt_motor
{
private:
    uint8_t _id;
    rs485_v3 *_rs485;
    agile_modbus_rtu_master_simple *_modbus;

    std::atomic<double> _des_pos_at;

    yj_return _rs485_flag;

public:
    typedef enum
    {
        motor_pos_mode_relative = 0x00,
        motor_pos_mode_absolute = 0x01,
    } motor_pos_mode_en;

    typedef enum
    {
        motor_zero_mode_single_near = 0x00,
        motor_zero_mode_single_with_direction = 0x01,
        motor_zero_mode_mutil = 0x02,
        motor_zero_mode_mutil_with_switch = 0x03

    } motor_zero_mode_en;

    typedef struct
    {
        uint32_t speed;
        double angle;
        bool is_back_to_zero;
        bool is_fail_back_to_zero;
        bool is_enable;
        bool is_in_position;
        bool is_stall;
        bool is_stall_protect;
    } motor_status_t, *Pmotor_status_t;

    inline yj_return _read_input_reg(uint32_t reg_addr, std::vector<uint16_t> &reg_data);

public:
    zdt_motor(uint8_t id, rs485_v3 *rs485, agile_modbus_rtu_master_simple *modbus);
    ~zdt_motor(){};

    yj_return set_speed(
        int32_t speed,
        uint8_t accel,
        bool is_sync = false);

    yj_return set_position(
        int32_t speed,
        uint8_t accel,
        double circle_num,
        bool is_sync = false,
        motor_pos_mode_en pos_mode = motor_pos_mode_relative,
        bool is_waiting = false // 是否等待到位
        );

    yj_return set_zero(motor_zero_mode_en mode = motor_zero_mode_mutil , bool is_sync = false);
    yj_return stop_zero();

    yj_return read_position(double &position); // 单位°
    yj_return read_speed(int32_t &speed);      // 单位 rpm
    yj_return read_zero_status(bool &is_zero,bool &is_zero_ok);      
    yj_return read_is_pos_ok(double death_zone_angle = 8,uint64_t timeout_ms = 3000); // 超时时间单位ms
    
    yj_return start_sync(bool is_broadcast = true); // 默认广播
    yj_return stop_motor(bool is_sync = false,bool is_broadcast = false); // 默认不广播
    yj_return clear_pos(); 
    yj_return clear_protect();
    
    
    yj_return read_status(motor_status_t &status); //无效
};